/**
 * @author Russell Toris - rctoris@wpi.edu
 * @author Lars Kunze - l.kunze@cs.bham.ac.uk
 * @author Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
 */

/**
 * A navigator can be used to add click-to-navigate options to an object. If
 * withOrientation is set to true, the user can also specify the orientation of
 * the robot by clicking at the goal position and pointing into the desired
 * direction (while holding the button pressed).
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * tfClient (optional) - the TF client
 *   * robot_pose (optional) - the robot topic or TF to listen position
 *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
 *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
 *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
 *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
 */
NAV.Navigator = function(options) {
        var that = this;
        options = options || {};
        var ros = options.ros;
        var tfClient = options.tfClient || null;
        var robot_pose = options.robot_pose || '/robot_pose';
        var serverName = options.serverName || '/move_base';
        var actionName = options.actionName || 'move_base_msgs/MoveBaseAction';
        var withOrientation = options.withOrientation || false;
        var use_image = options.image;
        this.rootObject = options.rootObject || new createjs.Container();

        this.goalMarker = null;


        // setup the actionlib client
        var actionClient = new ROSLIB.ActionClient({
            ros: ros,
            actionName: actionName,
            serverName: serverName
        });


        // PATH INIT
        this.planPath = this.planPath || null;
        var pathListener = new ROSLIB.Topic({
            ros: ros,
            name: '/move_base/NavfnROS/plan',
            messageType: 'nav_msgs/Path',
            throttle_rate: NAV.THROTTLE_RATE
        });
        // that.rootObject.addChild(that.planPath);

        // this.rootObject.addChild(this.planPath);
        var updatePath = function(plan) {
            if (!that.planPath) {
                that.planPath = new ROS2D.PathShape({ path: plan, strokeSize: 0.03, strokeColor: createjs.Graphics.getRGB(94, 82, 125, 0.7) })
                that.rootObject.addChild(that.planPath);
            } else {
                that.rootObject.removeChild(that.planPath);
                that.planPath.setPath(plan);
                that.rootObject.addChild(that.planPath);
            }
        }
        pathListener.subscribe(function(plan) {
            if (plan.poses && plan.poses.length > 5) {
                updatePath(plan)
            }
        });


        /**
         * Send a goal to the navigation stack with the given pose.
         *
         * @param pose - the goal pose
         */
        function sendGoal(pose) {
            // create a goal
            var goal = new ROSLIB.Goal({
                actionClient: actionClient,
                goalMessage: {
                    target_pose: {
                        header: {
                            frame_id: 'map'
                        },
                        pose: pose
                    }
                }
            });
            goal.send();

            that.currentGoal = goal;

            // create a marker for the goal
            if (that.goalMarker === null) {
                console.log('init goal')
                if (use_image && ROS2D.hasOwnProperty('ImageNavigator')) {
                    that.goalMarker = new ROS2D.ImageNavigator({
                        size: 2.5,
                        image: use_image,
                        alpha: 0.7,
                        pulse: true
                    });
                } else {
                    that.goalMarker = new RosCanvas.goalPoint({ size: 8, pulse: true });
                }
                // that.rootObject.addChild(that.goalMarker);
            }
            that.rootObject.addChild(that.goalMarker);
            that.goalMarker.x = pose.position.x;
            that.goalMarker.y = -pose.position.y;
            that.goalMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);
            that.goalMarker.scaleX = 1.0 / stage.scaleX;
            that.goalMarker.scaleY = 1.0 / stage.scaleY;

            goal.on('result', function() {
                // TODO
                if (that.planPath) {
                    that.rootObject.removeChild(that.planPath)
                }

                // that.rootObject.removeChild(that.goalMarker);
                // that.goalMarker = null
            });
        }

        /**
         * Cancel the currently active goal.
         */
        this.cancelGoal = function() {
            if (typeof that.currentGoal !== 'undefined') {
                that.currentGoal.cancel();
            }
        };

        // get a handle to the stage
        var stage;
        if (that.rootObject instanceof createjs.Stage) {
            stage = that.rootObject;
        } else {
            stage = that.rootObject.getStage();
        }

        // marker for the robot
        var robotMarker = null;
        if (use_image && ROS2D.hasOwnProperty('ImageNavigator')) {
            robotMarker = new ROS2D.ImageNavigator({
                size: 2.0,
                image: use_image,
                pulse: true
            });
        } else {
            robotMarker = new ROS2D.NavigationArrow({
                size: 20,
                strokeSize: 0.1,
                fillColor: "#583c8a",
                pulse: true
            });
        }

        // wait for a pose to come in first
        robotMarker.visible = false;
        this.rootObject.addChild(robotMarker);
        var initScaleSet = false;

        var updateRobotPosition = function(pose, orientation) {
            // update the robots position on the map
            robotMarker.x = pose.x;
            robotMarker.y = -pose.y;
            if (!initScaleSet) {
                robotMarker.scaleX = 1.0 / stage.scaleX;
                robotMarker.scaleY = 1.0 / stage.scaleY;
                initScaleSet = true;
            }
            // change the angle
            robotMarker.rotation = stage.rosQuaternionToGlobalTheta(orientation);
            // Set visible
            robotMarker.visible = true;
        };

        if (tfClient !== null) {
            tfClient.subscribe(robot_pose, function(tf) {
                // console.log("subscribe robot_pose tf:", tf)
                updateRobotPosition(tf.translation, tf.rotation);
            });
        } else {
            // setup a listener for the robot pose
            var poseListener = new ROSLIB.Topic({
                ros: ros,
                name: robot_pose,
                messageType: 'geometry_msgs/Pose',
                throttle_rate: NAV.THROTTLE_RATE
            });
            poseListener.subscribe(function(pose) {
                updateRobotPosition(pose.position, pose.orientation);
            });
        }


        if (withOrientation === false) {
            //病房坐标
            var testinput = [{
                    id: 1,
                    x: 844,
                    y: 128,
                    rom: '301',
                },
                {
                    id: 2,
                    x: 672,
                    y: 467,
                    rom: '302',
                },
                {
                    id: 3,
                    x: 545,
                    y: 168,
                    rom: '303',
                },
                {
                    id: 4,
                    x: 298,
                    y: 464,
                    rom: '304',
                }
            ];

            let num = 0;
            var div = document.getElementById("nav");
            div.onmousedown = function(event) {
                event = event || window.event;
                var pagex = event.pageX || scroll().left + event.clientX;
                var pagey = event.pageY || scroll().top + event.clientY;
                var divx = div.offsetLeft;
                var divy = div.offsetTop;
                var targetx = pagex - divx;
                var targety = pagey - divy;
                console.log(targetx + "," + targety);

                if (num < 4) {
                    let test = {
                        id: num + 1,
                        x: targetx,
                        y: targety,
                        rom: '30' + new String(num + 1)
                    }
                    testinput[num] = test
                    num++
                }
                console.log(testinput);

                // let boxA = document.querySelector('.boxA');
                // boxA.style.top = targety;
                // boxA.style.left = targetx;
                // console.log(boxA);
            }
            init();

            function init() {
                document.addEventListener("mousedown", mouseHandler);
            }

            let numz = 301
            function mouseHandler(e) {
                    var randomDiv = Method.$c("box", document.querySelector('#nav'), {
                        width: "50px",
                        height: "50px",
                        position: "absolute",
                        backgroundColor: `${divColor()}20`,
                        // opacity: 0.2,
                        borderRadius: '50%',
                        display: "flex",
                        'justify-content': "center",
                        'align-items': 'center'
                    })
                    
                    let p = document.createElement("p");
                    p.style.color="#000"
                    p.innerText = `${numz++}`
                    randomDiv.appendChild(p) ;
                    randomDiv.style.left = e.clientX - randomDiv.offsetWidth / 2 + "px"; 
                    randomDiv.style.top = e.clientY - randomDiv.offsetHeight / 2 + "px";
                    /*      top:e.clientY-this.offsetHeight/2+"px",//原因 设置为了X...xbl
                    //      removeEventListener(randomDiv);*/
                }

                function divColor() {
                    var col = "#"; //这个字符串第一位为# 颜色的格式
                    for (var i = 0; i < 6; i++) {
                        col += parseInt(Math.random() * 16).toString(16); //rondom*16后的随机值即为0-1*16==0-16;  toString(16)为转化为16进制
                    }
                    return col; //最后返回一个七位的值 格式即为#nnnnnn 颜色的格式
                }

                var testinput2 = [{
                        id: 1,
                        rom: '内科',
                    },
                    {
                        id: 2,
                        rom: '外科',
                    },
                    {
                        id: 3,
                        rom: '麻醉科',
                    },
                    {
                        id: 4,
                        rom: '检验科',
                    },
                    {
                        id: 5,
                        rom: '放射科',
                    },
                    {
                        id: 6,
                        rom: '皮肤科',
                    }
                ];
                var btutest = document.querySelector('.layui-btn');

                var inputrom = document.querySelector('.inputtest1'); //对应病房号
                var inputoffice = document.querySelector('.inputtest2'); //对应床号

                var logtest = document.querySelector('.log');
                var idtest = document.querySelector('#myid');
                var select1 = inputrom.children[0]; //对应病房的选择孩子编号
                var select2 = inputoffice.children[0]; //对应床号的选择孩子编号
                console.log('34234234', idtest);
                btutest.addEventListener('click', function() {
                    var msglog =
                        getDates() +
                        "/病房号:" +
                        testinput[select1.value - 1].rom +
                        "/科室:" +
                        testinput2[select2.value - 1].rom +
                        "/医师:" +
                        idtest.innerHTML;
                    var newLog =
                        '<div class="layui-card">' +
                        '    <div class="layui-card-header">' + getDates() + '</div>' +
                        '<div class="layui-card-body">' +
                        "病房号:" +
                        testinput[select1.value - 1].rom + '<br>' +
                        "科室:" +
                        testinput2[select2.value - 1].rom + '<br>' +
                        "医师:" +
                        idtest.innerHTML +
                        "</div>" +
                        '</div>';
                    // convert to ROS coordinates
                    if (that.planPath) {
                        that.rootObject.removeChild(that.planPath)
                    }
                    if (select1.value === "") return; //两个数据必须选上
                    var coords = stage.globalToRos(testinput[select1.value - 1].x, testinput[select1.value - 1].y);
                    // var coords = stage.globalToRos(100, 400);
                    var pose = new ROSLIB.Pose({
                        position: new ROSLIB.Vector3(coords)
                    });

                    // send the goal
                    sendGoal(pose);
                    // console.log(logtest);
                    logtest.insertAdjacentHTML('beforeend', newLog);
                    $.get('http://127.0.0.1:5000/check/superuser')
                    $.get('http://127.0.0.1:5000/check/user?name=' + msglog)
                    const logEle = document.getElementsByClassName("log")[0]
                    logEle.scrollTop = logEle.scrollHeight
                })

                function getDates() {
                    //设置当前时间
                    var date = new Date();
                    var year = date.getFullYear(); //月份从0~11，所以加一
                    var dateArr = [date.getMonth() + 1, date.getDate(), date.getHours(), date.getMinutes(), date.getSeconds()];
                    for (var i = 0; i < dateArr.length; i++) {
                        if (dateArr[i] >= 1 && dateArr[i] <= 9) {
                            dateArr[i] = "0" + dateArr[i];
                        }
                    }
                    var strDate = year + '-' + dateArr[0] + '-' + dateArr[1] + ' ' + dateArr[2] + ':' + dateArr[3] + ':' + dateArr[4];
                    return strDate;
                }

                this.rootObject.addEventListener('dblclick', function(event) {
                    // console.log(window.pageXOffset);
                    // convert to ROS coordinates
                    if (that.planPath) {
                        that.rootObject.removeChild(that.planPath)
                    }
                    console.log(event.stageX, event.stageY)
                    var coords = stage.globalToRos(event.stageX, event.stageY);
                    var pose = new ROSLIB.Pose({
                        position: new ROSLIB.Vector3(coords)
                    });

                    // send the goal
                    sendGoal(pose);
                });

            } else { // withOrientation === true
                // setup a click-and-point listener (with orientation)
                var position = null;
                var positionVec3 = null;
                var thetaRadians = 0;
                var thetaDegrees = 0;
                var orientationMarker = null;
                var mouseDown = false;
                var xDelta = 0;
                var yDelta = 0;

                var mouseEventHandler = function(event, mouseState) {

                    if (mouseState === 'down') {
                        // get position when mouse button is pressed down
                        position = stage.globalToRos(event.stageX, event.stageY);
                        positionVec3 = new ROSLIB.Vector3(position);
                        mouseDown = true;
                    } else if (mouseState === 'move') {
                        // remove obsolete orientation marker
                        that.rootObject.removeChild(orientationMarker);

                        if (mouseDown === true) {
                            // if mouse button is held down:
                            // - get current mouse position
                            // - calulate direction between stored <position> and current position
                            // - place orientation marker
                            var currentPos = stage.globalToRos(event.stageX, event.stageY);
                            var currentPosVec3 = new ROSLIB.Vector3(currentPos);

                            if (use_image && ROS2D.hasOwnProperty('ImageNavigator')) {
                                orientationMarker = new ROS2D.ImageNavigator({
                                    size: 2.5,
                                    image: use_image,
                                    alpha: 0.7,
                                    pulse: false
                                });
                            } else {
                                orientationMarker = new ROS2D.NavigationArrow({
                                    size: 25,
                                    strokeSize: 1,
                                    fillColor: createjs.Graphics.getRGB(0, 255, 0, 0.66),
                                    pulse: false
                                });
                            }

                            xDelta = currentPosVec3.x - positionVec3.x;
                            yDelta = currentPosVec3.y - positionVec3.y;

                            thetaRadians = Math.atan2(xDelta, yDelta);

                            thetaDegrees = thetaRadians * (180.0 / Math.PI);

                            if (thetaDegrees >= 0 && thetaDegrees <= 180) {
                                thetaDegrees += 270;
                            } else {
                                thetaDegrees -= 90;
                            }

                            orientationMarker.x = positionVec3.x;
                            orientationMarker.y = -positionVec3.y;
                            orientationMarker.rotation = thetaDegrees;
                            orientationMarker.scaleX = 1.0 / stage.scaleX;
                            orientationMarker.scaleY = 1.0 / stage.scaleY;

                            that.rootObject.addChild(orientationMarker);
                        }
                    } else if (mouseDown) { // mouseState === 'up'
                        // if mouse button is released
                        // - get current mouse position (goalPos)
                        // - calulate direction between stored <position> and goal position
                        // - set pose with orientation
                        // - send goal
                        mouseDown = false;

                        var goalPos = stage.globalToRos(event.stageX, event.stageY);

                        var goalPosVec3 = new ROSLIB.Vector3(goalPos);

                        xDelta = goalPosVec3.x - positionVec3.x;
                        yDelta = goalPosVec3.y - positionVec3.y;

                        thetaRadians = Math.atan2(xDelta, yDelta);

                        if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
                            thetaRadians += (3 * Math.PI / 2);
                        } else {
                            thetaRadians -= (Math.PI / 2);
                        }

                        var qz = Math.sin(-thetaRadians / 2.0);
                        var qw = Math.cos(-thetaRadians / 2.0);

                        var orientation = new ROSLIB.Quaternion({ x: 0, y: 0, z: qz, w: qw });

                        var pose = new ROSLIB.Pose({
                            position: positionVec3,
                            orientation: orientation
                        });
                        // send the goal
                        sendGoal(pose);
                    }
                };

                this.rootObject.addEventListener('stagemousedown', function(event) {
                    mouseEventHandler(event, 'down');
                });

                this.rootObject.addEventListener('stagemousemove', function(event) {
                    mouseEventHandler(event, 'move');
                });

                this.rootObject.addEventListener('stagemouseup', function(event) {
                    mouseEventHandler(event, 'up');
                });
            }
        };